#include "Vectors.h"
#include <cmath>
#include <cstdlib>
#include <string>
#include <new>

using namespace neiderra::math;

vec3::vec3(float X, float Y, float Z)
{
	x = X;
	y = Y;
	z = Z;
}

vec3::vec3()
{
	x = y = z = 0;
}

vec3 vec3::operator-()
{
	return vec3(-x, -y, -z);
}

vec3 vec3::operator+()
{
	return vec3(x, y, z);
}

vec3 vec3::operator+(vec3& v)
{
	return vec3(v.x+x, v.y+y, v.z+z);
}

vec3 vec3::operator-(vec3& v)
{
	return vec3(x-v.x, y-v.y, z-v.z);
}

// vec3 vec3::operator*(vec3& v1, vec3& v2)
// {
// 	return vec3(v1.y*v2.z - v1.z*v2.y,
// 				v1.z*v2.x - v1.x*v2.z,
// 				v1.x*v2.y - v1.y*v2.x);
// 
// }

// vec3 vec3::operator-(vec3 v)
// {
// 	return vec3(x-v.x,y-v.y,z-v.z);
// }

bool vec3::operator ==(float a)
{
	return (x == y == z == a);
}

bool vec3::operator !=(float a)
{
	return (x!=a) || (y!=a) || (z!=a);
}

vec3 vec3::operator/(float a)
{
    x /= a;
	y /= a;
	z /= a;
	return *this;
}

vec3& vec3::operator=(vec3 v)
{
	x = v.x;
	y = v.y;
	z = v.z;
	//return vec3(x, y, z);
	return *this;
}

void vec3::operator+=(vec3 v)
{
	x += v.x;
	y += v.y;
	z += v.z;
}

void vec3::operator-=(vec3 v)
{
	x -= v.x;
	y -= v.y;
	z -= v.z;
}

inline float vec3::mag()
{
	return std::sqrt(x*x + y*y + z*z);
}

/*vec3 vec3::normalize()
{
	float vmag = mag();

	if(vmag == 0) return vec3(1,0,0);

    float oneOverMag = 1.0f / vmag;

	return vec3(x * oneOverMag,
			y * oneOverMag,
			z * oneOverMag);
}*/

vec3 vec3::normalize()
{
	float vmag = mag();
	if (vmag == 0) { x=1.0f; y=0.0f; z = 0.0f;  return *this; }

    float oneOverMag = 1.0f / vmag;
	x *= oneOverMag;
	y *= oneOverMag;
	z *= oneOverMag;

	return *this;
}

void vec3::set(float X, float Y, float Z)
{
	x = X;
	y = Y;
	z = Z;
}

vec3 vec3::operator =(float a)
{
	x = a;
	y = a;
	z = a;
	return *this;
}

void vec3::operator *=(float a)
{
	x*=a;
	y*=a;
	z*=a;
}

void vec3::operator /=(float a)
{
	x/=a;
	y/=a;
	z/=a;
}

void vec3::operator -=(float a)
{
	x-=a;
	y-=a;
	z-=a;
}

void vec3::operator +=(float a)
{
	x+=a;
	y+=a;
	z+=a;
}

vec3 vec3::operator +(float a)
{
	return vec3(x + a, y + a, z + a);
}

vec3 vec3::operator *(float a)
{
	return vec3(x * a, y * a, z * a);
}

void neiderra::math::convertVecToTraslationMatrix(vec3& v, float* mat)
{
	mat[0] = mat[5] = mat[10] = mat[15] = 1.0f;
	mat[1] = mat[2] = mat[4] = mat[6] = mat[8] = mat[9] = mat[12] = mat[13] = 0.0f;
	mat[3] = v.x;
	mat[7] = v.y;
	mat[11] = v.z;
}

void neiderra::math::trim(std::string& str)
{
	using namespace std;
	string s = str;
	int i, j;
	for(i=0,j=0; i< str.size(); i++)
	{
		if(str[i] != ' '){
			s[j] = str[i];
			j++;
		}
	}
	str = s;

	str.resize(j);
	
  //string::size_type pos = str.find_last_not_of(' ');
  //if(pos != string::npos) {
  //  str.erase(pos + 1);
  //  pos = str.find_first_not_of(' ');
  //  if(pos != string::npos) str.erase(0, pos);
  //}
  //else str.erase(str.begin(), str.end());
}

void neiderra::math::convertStringToVec3(vec3& v, const char* str)
{
	using namespace std;
	string s = str;
	trim(s);

	char buf[10];
	for(int j=0; j<10; j++) buf[j] = 0;
	
	int i;
	
	for(i=0;s[i]!=',';i++);

	s.copy(buf, i, 0);
	v.x = atof(buf);
	for(int j=0; j<10; j++) buf[j] = 0;
	
	int oldi = i;

	for(++i;s[i]!=',';i++);

	s.copy(buf, i-oldi-1, oldi+1);
	v.y = atof(buf);
	for(int j=0; j<10; j++) buf[j] = 0;
	
	oldi = (i++);
	s.copy(buf, 8, oldi+1);
	v.z = atof(buf);
}

// vec3 vec3::operator *(vec3& v)
// {
//     return vec3(y*v.z - z*v.y,
// 		z*v.x - x*v.z,
// 		x*v.y - y*v.x);
// }

void vec3::constructor_vec3(vec3* v)
{
	new(v) vec3();
}

void vec3::destructor_vec3(vec3* v)
{
	v->~vec3();
}

float vec3::dotprod(vec3& v)
{
    //return mag()*v.mag()*cos(
	return 0.0f;
}

vec3 neiderra::math::operator*(vec3& v1, vec3& v2)
{
	return vec3(v1.y*v2.z - v1.z*v2.y,
				v1.z*v2.x - v1.x*v2.z,
				v1.x*v2.y - v1.y*v2.x);
}
